package BeamAndBall;


// PID class to be written by you
public class PID {

	// Private attribute containing a reference to the GUI
	@SuppressWarnings("unused")
	private PIDGUI GUI;
	// Private attribute containing a reference to the PIDParameters
	// currently used
	private PIDParameters pidp;

	// Additional attributes
	private double outerI; // Integrator state
	private double outerD; //Derivator state

	private double outerv; // Desired control signal
	private double outere; // Current control error

	private double ad,bd, outeryOld; // for D
	
	// Constructor
	public PID(String name) {
		PIDParameters p = new PIDParameters();
		p.outerBeta = 1.0;
		p.outerH = 0.01;
		p.outerintegratorOn = false;
		p.outerK = -0.1;
		p.outerTi = 0.0;
		p.outerTr = 10.0;
		p.outerTd = 1.7;
		p.outerN = 7;
		
		GUI = new PIDGUI(this, p, name);
		setParameters(p);

		this.outerI = 0.0;
		this.outerD = 0.0;
		
		this.ad = p.outerTd / (p.outerTd + p.outerN*p.outerH);
		this.bd = p.outerK*this.ad*p.outerN;
		
		this.outerv = 0.0;
		this.outere = 0.0;
	}

	// Calculates the control signal v. Called from BeamAndBallRegul.
	public synchronized double calculateOutput(double yball, double yref){
		this.outere = yref - yball;
		this.outerD = this.ad * this.outerD - this.bd * (yball - this.outeryOld);
		this.outerv = pidp.outerK * (pidp.outerBeta * yref - yball) + outerI + outerD; // I is 0.0 if integratorOn is false
		this.outeryOld = yball;
		return this.outerv;
	}

	// Updates the controller state. Should use tracking-based anti-windup
	// Called from BeamAndBallRegul.
	public synchronized void updateState(double u){
		  if (pidp.outerintegratorOn) {
			  outerI = outerI + (pidp.outerK * pidp.outerH / pidp.outerTi) * outere + (pidp.outerH / pidp.outerTr) * (u - this.outerv);
		  } else {
			  outerI = 0.0;
		  }
		  
	}

	// Returns the sampling interval expressed as a long. Explicit type casting
	// needed
	public synchronized long getHMillis() {
		return (long) (pidp.outerH * 1000.0);
	}

	// Sets the PIDParameters. Called from PIDGUI. Must clone newParameters.
	public synchronized void setParameters(PIDParameters newParameters){
		
		pidp = (PIDParameters)newParameters.clone();
		
		this.ad = pidp.outerTd / (pidp.outerTd + pidp.outerN*pidp.outerH);
		this.bd = pidp.outerK*this.ad*pidp.outerN;
		
	}

}